/*
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
*
* This file is part of VL53L1 Core and is dual licensed,
* either 'STMicroelectronics
* Proprietary license'
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
*
********************************************************************************
*
* 'STMicroelectronics Proprietary license'
*
********************************************************************************
*
* License terms: STMicroelectronics Proprietary in accordance with licensing
* terms at www.st.com/sla0081
*
* STMicroelectronics confidential
* Reproduction and Communication of this document is strictly prohibited unless
* specifically authorized in writing by STMicroelectronics.
*
*
********************************************************************************
*
* Alternatively, VL53L1 Core may be distributed under the terms of
* 'BSD 3-clause "New" or "Revised" License', in which case the following
* provisions apply instead of the ones mentioned above :
*
********************************************************************************
*
* License terms: BSD 3-clause "New" or "Revised" License.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
********************************************************************************
*
*/

/**
 * @file  vl53l1_core.c
 *
 * @brief EwokPlus25 core function definition
 */

#include "vl53l1_ll_def.h"
#include "vl53l1_ll_device.h"
#include "vl53l1_platform.h"
#include "vl53l1_register_map.h"
#include "vl53l1_register_funcs.h"
#include "vl53l1_register_settings.h"
#include "vl53l1_api_preset_modes.h"
#include "vl53l1_core.h"
#include "vl53l1_tuning_parm_defaults.h"

#ifdef VL53L1_LOGGING
#include "vl53l1_api_debug.h"
#include "vl53l1_debug.h"
#include "vl53l1_register_debug.h"
#endif

#define LOG_FUNCTION_START(fmt, ...) \
    _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
    _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...)      \
    _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
                          status, fmt, ##__VA_ARGS__)

#define trace_print(level, ...)                \
    _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
                     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)


void VL53L1_init_version(
    VL53L1_DEV Dev)
{
    /**
	 * Initialise version structure
	 */

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    pdev->version.ll_major    = VL53L1_LL_API_IMPLEMENTATION_VER_MAJOR;
    pdev->version.ll_minor    = VL53L1_LL_API_IMPLEMENTATION_VER_MINOR;
    pdev->version.ll_build    = VL53L1_LL_API_IMPLEMENTATION_VER_SUB;
    pdev->version.ll_revision = VL53L1_LL_API_IMPLEMENTATION_VER_REVISION;
}


void VL53L1_init_ll_driver_state(
    VL53L1_DEV         Dev,
    VL53L1_DeviceState device_state)
{
    /**
	 * Initialise LL Driver state variables
	 */

    VL53L1_LLDriverData_t    *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);
    VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);

    pstate->cfg_device_state  = device_state;
    pstate->cfg_stream_count  = 0;
    pstate->cfg_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
    pstate->cfg_timing_status = 0;

    pstate->rd_device_state  = device_state;
    pstate->rd_stream_count  = 0;
    pstate->rd_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
    pstate->rd_timing_status = 0;
}


VL53L1_Error VL53L1_update_ll_driver_rd_state(
    VL53L1_DEV Dev)
{
    /**
	 * State machine for read device state
	 *
	 * VL53L1_DEVICESTATE_SW_STANDBY
	 * VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC
	 * VL53L1_DEVICESTATE_RANGING_GATHER_DATA
	 * VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA
	 */

    VL53L1_Error              status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t    *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);
    VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);

    /* if top bits of mode start reset are zero then in standby state */

    LOG_FUNCTION_START("");

#ifdef VL53L1_LOGGING
    VL53L1_print_ll_driver_state(pstate);
#endif

    if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00)
    {
        pstate->rd_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
        pstate->rd_stream_count  = 0;
        pstate->rd_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
        pstate->rd_timing_status = 0;
    }
    else
    {
        /*
		 * implement read stream count
		 */

        if (pstate->rd_stream_count == 0xFF)
        {
            pstate->rd_stream_count = 0x80;
        }
        else
        {
            pstate->rd_stream_count++;
        }


        /*
		 * Toggle grouped parameter hold ID
		 */

        pstate->rd_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;

        /* Ok now ranging  */

        switch (pstate->rd_device_state)
        {
        case VL53L1_DEVICESTATE_SW_STANDBY:

            if ((pdev->dyn_cfg.system__grouped_parameter_hold & VL53L1_GROUPEDPARAMETERHOLD_ID_MASK) > 0)
            {
                pstate->rd_device_state =
                    VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC;
            }
            else
            {
                pstate->rd_device_state =
                    VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;
            }

            pstate->rd_stream_count  = 0;
            pstate->rd_timing_status = 0;

            break;

        case VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC:

            pstate->rd_stream_count = 0;
            pstate->rd_device_state =
                VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;

            break;

        case VL53L1_DEVICESTATE_RANGING_GATHER_DATA:

            pstate->rd_device_state =
                VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;

            break;

        case VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA:

            pstate->rd_timing_status ^= 0x01;

            pstate->rd_device_state =
                VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA;

            break;

        default:

            pstate->rd_device_state =
                VL53L1_DEVICESTATE_SW_STANDBY;
            pstate->rd_stream_count  = 0;
            pstate->rd_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
            pstate->rd_timing_status = 0;

            break;
        }
    }

#ifdef VL53L1_LOGGING
    VL53L1_print_ll_driver_state(pstate);
#endif

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_check_ll_driver_rd_state(
    VL53L1_DEV Dev)
{
    /*
	 * Checks if the LL Driver Read state and expected stream count
	 * matches the state and stream count received from the device
	 *
	 * Check is only use in back to back mode
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev =
        VL53L1DevStructGetLLDriverHandle(Dev);

    VL53L1_ll_driver_state_t *pstate       = &(pdev->ll_state);
    VL53L1_system_results_t  *psys_results = &(pdev->sys_results);

    uint8_t device_range_status = 0;
    uint8_t device_stream_count = 0;
    uint8_t device_gph_id       = 0;

    LOG_FUNCTION_START("");

#ifdef VL53L1_LOGGING
    VL53L1_print_ll_driver_state(pstate);
#endif

    device_range_status =
        psys_results->result__range_status & VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;

    device_stream_count = psys_results->result__stream_count;

    /* load the correct GPH ID */
    device_gph_id = (psys_results->result__interrupt_status & VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK) >> 4;

    /* only apply checks in back to back mode */

    if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK) == VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK)
    {
        /* if read state is wait for GPH sync interrupt then check the
		 * device returns a GPH range status value otherwise check that
		 * the stream count matches
		 *
		 * In theory the stream count should zero for the GPH interrupt
		 * but that is not the case after at abort ....
		 */

        if (pstate->rd_device_state == VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC)
        {
            if (device_range_status != VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY)
            {
                status = VL53L1_ERROR_GPH_SYNC_CHECK_FAIL;
            }
        }
        else
        {
            if (pstate->rd_stream_count != device_stream_count)
            {
                status = VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL;
            }

            /*
		 * Check Read state GPH ID
		 */

            if (pstate->rd_gph_id != device_gph_id)
            {
                status = VL53L1_ERROR_GPH_ID_CHECK_FAIL;
#ifdef VL53L1_LOGGING
                trace_print(VL53L1_TRACE_LEVEL_ALL,
                            "    RDSTATECHECK: Check failed: rd_gph_id: %d, device_gph_id: %d\n",
                            pstate->rd_gph_id,
                            device_gph_id);
#endif
            }
            else
            {
#ifdef VL53L1_LOGGING
                trace_print(VL53L1_TRACE_LEVEL_ALL,
                            "    RDSTATECHECK: Check passed: rd_gph_id: %d, device_gph_id: %d\n",
                            pstate->rd_gph_id,
                            device_gph_id);
#endif
            }

        } /* else (not in WAIT_GPH_SYNC) */

    } /* if back to back */

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_update_ll_driver_cfg_state(
    VL53L1_DEV Dev)
{
    /**
	 * State machine for configuration device state
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev =
        VL53L1DevStructGetLLDriverHandle(Dev);

    VL53L1_ll_driver_state_t *pstate = &(pdev->ll_state);

    LOG_FUNCTION_START("");

#ifdef VL53L1_LOGGING
    VL53L1_print_ll_driver_state(pstate);
#endif

    /* if top bits of mode start reset are zero then in standby state */

    if ((pdev->sys_ctrl.system__mode_start & VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK) == 0x00)
    {
        pstate->cfg_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
        pstate->cfg_stream_count  = 0;
        pstate->cfg_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
        pstate->cfg_timing_status = 0;
    }
    else
    {
        /*
		 * implement configuration stream count
		 */

        if (pstate->cfg_stream_count == 0xFF)
        {
            pstate->cfg_stream_count = 0x80;
        }
        else
        {
            pstate->cfg_stream_count++;
        }

        /*
		 * Toggle grouped parameter hold ID
		 */

        pstate->cfg_gph_id ^= VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;

        /*
		 * Implement configuration state machine
		 */

        switch (pstate->cfg_device_state)
        {
        case VL53L1_DEVICESTATE_SW_STANDBY:

            pstate->cfg_timing_status ^= 0x01;
            pstate->cfg_stream_count   = 1;

            pstate->cfg_device_state = VL53L1_DEVICESTATE_RANGING_DSS_AUTO;
            break;

        case VL53L1_DEVICESTATE_RANGING_DSS_AUTO:

            pstate->cfg_timing_status ^= 0x01;

            break;

        default:

            pstate->cfg_device_state  = VL53L1_DEVICESTATE_SW_STANDBY;
            pstate->cfg_stream_count  = 0;
            pstate->cfg_gph_id        = VL53L1_GROUPEDPARAMETERHOLD_ID_MASK;
            pstate->cfg_timing_status = 0;

            break;
        }
    }

#ifdef VL53L1_LOGGING
    VL53L1_print_ll_driver_state(pstate);
#endif

    LOG_FUNCTION_END(status);

    return status;
}


void VL53L1_copy_rtn_good_spads_to_buffer(
    VL53L1_nvm_copy_data_t *pdata,
    uint8_t                *pbuffer)
{
    /*
	 * Convenience function to copy return SPAD enables to buffer
	 */

    *(pbuffer + 0)  = pdata->global_config__spad_enables_rtn_0;
    *(pbuffer + 1)  = pdata->global_config__spad_enables_rtn_1;
    *(pbuffer + 2)  = pdata->global_config__spad_enables_rtn_2;
    *(pbuffer + 3)  = pdata->global_config__spad_enables_rtn_3;
    *(pbuffer + 4)  = pdata->global_config__spad_enables_rtn_4;
    *(pbuffer + 5)  = pdata->global_config__spad_enables_rtn_5;
    *(pbuffer + 6)  = pdata->global_config__spad_enables_rtn_6;
    *(pbuffer + 7)  = pdata->global_config__spad_enables_rtn_7;
    *(pbuffer + 8)  = pdata->global_config__spad_enables_rtn_8;
    *(pbuffer + 9)  = pdata->global_config__spad_enables_rtn_9;
    *(pbuffer + 10) = pdata->global_config__spad_enables_rtn_10;
    *(pbuffer + 11) = pdata->global_config__spad_enables_rtn_11;
    *(pbuffer + 12) = pdata->global_config__spad_enables_rtn_12;
    *(pbuffer + 13) = pdata->global_config__spad_enables_rtn_13;
    *(pbuffer + 14) = pdata->global_config__spad_enables_rtn_14;
    *(pbuffer + 15) = pdata->global_config__spad_enables_rtn_15;
    *(pbuffer + 16) = pdata->global_config__spad_enables_rtn_16;
    *(pbuffer + 17) = pdata->global_config__spad_enables_rtn_17;
    *(pbuffer + 18) = pdata->global_config__spad_enables_rtn_18;
    *(pbuffer + 19) = pdata->global_config__spad_enables_rtn_19;
    *(pbuffer + 20) = pdata->global_config__spad_enables_rtn_20;
    *(pbuffer + 21) = pdata->global_config__spad_enables_rtn_21;
    *(pbuffer + 22) = pdata->global_config__spad_enables_rtn_22;
    *(pbuffer + 23) = pdata->global_config__spad_enables_rtn_23;
    *(pbuffer + 24) = pdata->global_config__spad_enables_rtn_24;
    *(pbuffer + 25) = pdata->global_config__spad_enables_rtn_25;
    *(pbuffer + 26) = pdata->global_config__spad_enables_rtn_26;
    *(pbuffer + 27) = pdata->global_config__spad_enables_rtn_27;
    *(pbuffer + 28) = pdata->global_config__spad_enables_rtn_28;
    *(pbuffer + 29) = pdata->global_config__spad_enables_rtn_29;
    *(pbuffer + 30) = pdata->global_config__spad_enables_rtn_30;
    *(pbuffer + 31) = pdata->global_config__spad_enables_rtn_31;
}


void VL53L1_init_system_results(
    VL53L1_system_results_t *pdata)
{
    /*
	 * Initialises the system results to all 0xFF just like the
	 * device firmware does a the start of a range
	 */

    pdata->result__interrupt_status = 0xFF;
    pdata->result__range_status     = 0xFF;
    pdata->result__report_status    = 0xFF;
    pdata->result__stream_count     = 0xFF;

    pdata->result__dss_actual_effective_spads_sd0         = 0xFFFF;
    pdata->result__peak_signal_count_rate_mcps_sd0        = 0xFFFF;
    pdata->result__ambient_count_rate_mcps_sd0            = 0xFFFF;
    pdata->result__sigma_sd0                              = 0xFFFF;
    pdata->result__phase_sd0                              = 0xFFFF;
    pdata->result__final_crosstalk_corrected_range_mm_sd0 = 0xFFFF;
    pdata->result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 =
        0xFFFF;
    pdata->result__mm_inner_actual_effective_spads_sd0 = 0xFFFF;
    pdata->result__mm_outer_actual_effective_spads_sd0 = 0xFFFF;
    pdata->result__avg_signal_count_rate_mcps_sd0      = 0xFFFF;

    pdata->result__dss_actual_effective_spads_sd1         = 0xFFFF;
    pdata->result__peak_signal_count_rate_mcps_sd1        = 0xFFFF;
    pdata->result__ambient_count_rate_mcps_sd1            = 0xFFFF;
    pdata->result__sigma_sd1                              = 0xFFFF;
    pdata->result__phase_sd1                              = 0xFFFF;
    pdata->result__final_crosstalk_corrected_range_mm_sd1 = 0xFFFF;
    pdata->result__spare_0_sd1                            = 0xFFFF;
    pdata->result__spare_1_sd1                            = 0xFFFF;
    pdata->result__spare_2_sd1                            = 0xFFFF;
    pdata->result__spare_3_sd1                            = 0xFF;
}


void VL53L1_i2c_encode_uint16_t(
    uint16_t ip_value,
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Encodes a uint16_t register value into an I2C write buffer
	 * MS byte first order (as per I2C register map.
	 */

    uint16_t i    = 0;
    uint16_t data = 0;

    data = ip_value;

    for (i = 0; i < count; i++)
    {
        pbuffer[count - i - 1] = (uint8_t)(data & 0x00FF);
        data                   = data >> 8;
    }
}

uint16_t VL53L1_i2c_decode_uint16_t(
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Decodes a uint16_t from the input I2C read buffer
	 * (MS byte first order)
	 */

    uint16_t value = 0x00;

    while (count-- > 0)
    {
        value = (value << 8) | (uint16_t)*pbuffer++;
    }

    return value;
}


void VL53L1_i2c_encode_int16_t(
    int16_t  ip_value,
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Encodes a int16_t register value into an I2C write buffer
	 * MS byte first order (as per I2C register map.
	 */

    uint16_t i    = 0;
    int16_t  data = 0;

    data = ip_value;

    for (i = 0; i < count; i++)
    {
        pbuffer[count - i - 1] = (uint8_t)(data & 0x00FF);
        data                   = data >> 8;
    }
}

int16_t VL53L1_i2c_decode_int16_t(
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Decodes a int16_t from the input I2C read buffer
	 * (MS byte first order)
	 */

    int16_t value = 0x00;

    /* implement sign extension */
    if (*pbuffer >= 0x80)
    {
        value = 0xFFFF;
    }

    while (count-- > 0)
    {
        value = (value << 8) | (int16_t)*pbuffer++;
    }

    return value;
}

void VL53L1_i2c_encode_uint32_t(
    uint32_t ip_value,
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Encodes a uint32_t register value into an I2C write buffer
	 * MS byte first order (as per I2C register map.
	 */

    uint16_t i    = 0;
    uint32_t data = 0;

    data = ip_value;

    for (i = 0; i < count; i++)
    {
        pbuffer[count - i - 1] = (uint8_t)(data & 0x00FF);
        data                   = data >> 8;
    }
}

uint32_t VL53L1_i2c_decode_uint32_t(
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Decodes a uint32_t from the input I2C read buffer
	 * (MS byte first order)
	 */

    uint32_t value = 0x00;

    while (count-- > 0)
    {
        value = (value << 8) | (uint32_t)*pbuffer++;
    }

    return value;
}


uint32_t VL53L1_i2c_decode_with_mask(
    uint16_t count,
    uint8_t *pbuffer,
    uint32_t bit_mask,
    uint32_t down_shift,
    uint32_t offset)
{
    /*
	 * Decodes an integer from the input I2C read buffer
	 * (MS byte first order)
	 */

    uint32_t value = 0x00;

    /* extract from buffer */
    while (count-- > 0)
    {
        value = (value << 8) | (uint32_t)*pbuffer++;
    }

    /* Apply bit mask and down shift */
    value = value & bit_mask;
    if (down_shift > 0)
    {
        value = value >> down_shift;
    }

    /* add offset */
    value = value + offset;

    return value;
}


void VL53L1_i2c_encode_int32_t(
    int32_t  ip_value,
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Encodes a int32_t register value into an I2C write buffer
	 * MS byte first order (as per I2C register map.
	 */

    uint16_t i    = 0;
    int32_t  data = 0;

    data = ip_value;

    for (i = 0; i < count; i++)
    {
        pbuffer[count - i - 1] = (uint8_t)(data & 0x00FF);
        data                   = data >> 8;
    }
}

int32_t VL53L1_i2c_decode_int32_t(
    uint16_t count,
    uint8_t *pbuffer)
{
    /*
	 * Decodes a int32_t from the input I2C read buffer
	 * (MS byte first order)
	 */

    int32_t value = 0x00;

    /* implement sign extension */
    if (*pbuffer >= 0x80)
    {
        value = 0xFFFFFFFF;
    }

    while (count-- > 0)
    {
        value = (value << 8) | (int32_t)*pbuffer++;
    }

    return value;
}


#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_start_test(
    VL53L1_DEV Dev,
    uint8_t    test_mode__ctrl)
{
    /*
	 * Triggers the start of a test mode
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    if (status == VL53L1_ERROR_NONE)
    { /*lint !e774 always true*/
        status = VL53L1_WrByte(
            Dev,
            VL53L1_TEST_MODE__CTRL,
            test_mode__ctrl);
    }

    LOG_FUNCTION_END(status);

    return status;
}
#endif


VL53L1_Error VL53L1_set_firmware_enable_register(
    VL53L1_DEV Dev,
    uint8_t    value)
{
    /*
	 * Set FIRMWARE__ENABLE register
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    pdev->sys_ctrl.firmware__enable = value;

    status = VL53L1_WrByte(
        Dev,
        VL53L1_FIRMWARE__ENABLE,
        pdev->sys_ctrl.firmware__enable);

    return status;
}

VL53L1_Error VL53L1_enable_firmware(
    VL53L1_DEV Dev)
{
    /*
	 * Enable firmware
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    status = VL53L1_set_firmware_enable_register(Dev, 0x01);

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_disable_firmware(
    VL53L1_DEV Dev)
{
    /*
	 * Disable firmware
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    status = VL53L1_set_firmware_enable_register(Dev, 0x00);

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_set_powerforce_register(
    VL53L1_DEV Dev,
    uint8_t    value)
{
    /*
	 * Set power force register
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    pdev->sys_ctrl.power_management__go1_power_force = value;

    status = VL53L1_WrByte(
        Dev,
        VL53L1_POWER_MANAGEMENT__GO1_POWER_FORCE,
        pdev->sys_ctrl.power_management__go1_power_force);

    return status;
}


VL53L1_Error VL53L1_enable_powerforce(
    VL53L1_DEV Dev)
{
    /*
	 * Enable power force
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    status = VL53L1_set_powerforce_register(Dev, 0x01);

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_disable_powerforce(
    VL53L1_DEV Dev)
{
    /*
	 * Disable power force
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    status = VL53L1_set_powerforce_register(Dev, 0x00);

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_clear_interrupt(
    VL53L1_DEV Dev)
{
    /*
	 * Clear Ranging interrupt by writing to
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    pdev->sys_ctrl.system__interrupt_clear = VL53L1_CLEAR_RANGE_INT;

    status = VL53L1_WrByte(
        Dev,
        VL53L1_SYSTEM__INTERRUPT_CLEAR,
        pdev->sys_ctrl.system__interrupt_clear);

    LOG_FUNCTION_END(status);

    return status;
}


#ifdef VL53L1_DEBUG
VL53L1_Error VL53L1_force_shadow_stream_count_to_zero(
    VL53L1_DEV Dev)
{
    /*
	 * Forces shadow stream count to zero
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    if (status == VL53L1_ERROR_NONE)
    { /*lint !e774 always true*/
        status = VL53L1_disable_firmware(Dev);
    }

    if (status == VL53L1_ERROR_NONE)
    {
        status = VL53L1_WrByte(
            Dev,
            VL53L1_SHADOW_RESULT__STREAM_COUNT,
            0x00);
    }

    if (status == VL53L1_ERROR_NONE)
    {
        status = VL53L1_enable_firmware(Dev);
    }

    return status;
}
#endif

uint32_t VL53L1_calc_macro_period_us(
    uint16_t fast_osc_frequency,
    uint8_t  vcsel_period)
{
    /* Calculates macro period in [us] from the input fast oscillator
	 * frequency and VCSEL period
	 *
	 * Macro period fixed point format = unsigned 12.12
	 * Maximum supported macro period  = 4095.9999 us
	 */

    uint32_t pll_period_us      = 0;
    uint8_t  vcsel_period_pclks = 0;
    uint32_t macro_period_us    = 0;

    LOG_FUNCTION_START("");

    /*  Calculate PLL period in [us] from the  fast_osc_frequency
	 *  Fast osc frequency fixed point format = unsigned 4.12
	 */

    pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency);

    /*  VCSEL period
	 *  - the real VCSEL period in PLL clocks = 2*(VCSEL_PERIOD+1)
	 */

    vcsel_period_pclks = VL53L1_decode_vcsel_period(vcsel_period);

    /*  Macro period
	 *  - PLL period [us]      = 0.24 format
	 *      - for 1.0 MHz fast oscillator freq
	 *      - max PLL period = 1/64 (6-bits)
	 *      - i.e only the lower 18-bits of PLL Period value are used
	 *  - Macro period [vclks] = 2304 (12-bits)
	 *
	 *  Max bits (24 - 6) + 12 = 30-bits usage
	 *
	 *  Downshift by 6 before multiplying by the VCSEL Period
	 */

    macro_period_us =
        (uint32_t)VL53L1_MACRO_PERIOD_VCSEL_PERIODS * pll_period_us;
    macro_period_us = macro_period_us >> 6;

    macro_period_us = macro_period_us * (uint32_t)vcsel_period_pclks;
    macro_period_us = macro_period_us >> 6;

#ifdef VL53L1_LOGGING
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u\n", "pll_period_us",
                pll_period_us);
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u\n", "vcsel_period_pclks",
                vcsel_period_pclks);
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u\n", "macro_period_us",
                macro_period_us);
#endif

    LOG_FUNCTION_END(0);

    return macro_period_us;
}


uint16_t VL53L1_calc_range_ignore_threshold(
    uint32_t central_rate,
    int16_t  x_gradient,
    int16_t  y_gradient,
    uint8_t  rate_mult)
{
    /* Calculates Range Ignore Threshold rate per spad
	 * in Mcps - 3.13 format
	 *
	 * Calculates worst case xtalk rate per spad in array corner
	 * based on input central xtalk and x and y gradients
	 *
	 * Worst case rate = central rate + (8*(magnitude(xgrad)) +
	 * (8*(magnitude(ygrad)))
	 *
	 * Range ignore threshold rate is then multiplied by user input
	 * rate_mult (in 3.5 fractional format)
	 *
	 */

    int32_t  range_ignore_thresh_int  = 0;
    uint16_t range_ignore_thresh_kcps = 0;
    int32_t  central_rate_int         = 0;
    int16_t  x_gradient_int           = 0;
    int16_t  y_gradient_int           = 0;

    LOG_FUNCTION_START("");

    /* Shift central_rate to .13 fractional for simple addition */

    central_rate_int = ((int32_t)central_rate * (1 << 4)) / (1000);

    if (x_gradient < 0)
    {
        x_gradient_int = x_gradient * -1;
    }

    if (y_gradient < 0)
    {
        y_gradient_int = y_gradient * -1;
    }

    /* Calculate full rate per spad - worst case from measured xtalk */
    /* Generated here from .11 fractional kcps */
    /* Additional factor of 4 applied to bring fractional precision to .13 */

    range_ignore_thresh_int = (8 * x_gradient_int * 4) + (8 * y_gradient_int * 4);

    /* Convert Kcps to Mcps */

    range_ignore_thresh_int = range_ignore_thresh_int / 1000;

    /* Combine with Central Rate - Mcps .13 format*/

    range_ignore_thresh_int = range_ignore_thresh_int + central_rate_int;

    /* Mult by user input */

    range_ignore_thresh_int = (int32_t)rate_mult * range_ignore_thresh_int;

    range_ignore_thresh_int = (range_ignore_thresh_int + (1 << 4)) / (1 << 5);

    /* Finally clip and output in correct format */

    if (range_ignore_thresh_int > 0xFFFF)
    {
        range_ignore_thresh_kcps = 0xFFFF;
    }
    else
    {
        range_ignore_thresh_kcps = (uint16_t)range_ignore_thresh_int;
    }

#ifdef VL53L1_LOGGING
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u\n", "range_ignore_thresh_kcps",
                range_ignore_thresh_kcps);
#endif

    LOG_FUNCTION_END(0);

    return range_ignore_thresh_kcps;
}


uint32_t VL53L1_calc_timeout_mclks(
    uint32_t timeout_us,
    uint32_t macro_period_us)
{
    /*  Calculates the timeout value in macro periods based on the input
	 *  timeout period in milliseconds and the macro period in [us]
	 *
	 *  Max timeout supported is 1000000 us (1 sec) -> 20-bits
	 *  Max timeout in 20.12 format = 32-bits
	 *
	 *  Macro period [us] = 12.12 format
	 */

    uint32_t timeout_mclks = 0;

    LOG_FUNCTION_START("");

    timeout_mclks =
        ((timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us;

    LOG_FUNCTION_END(0);

    return timeout_mclks;
}


uint16_t VL53L1_calc_encoded_timeout(
    uint32_t timeout_us,
    uint32_t macro_period_us)
{
    /*  Calculates the encoded timeout register value based on the input
	 *  timeout period in milliseconds and the macro period in [us]
	 *
	 *  Max timeout supported is 1000000 us (1 sec) -> 20-bits
	 *  Max timeout in 20.12 format = 32-bits
	 *
	 *  Macro period [us] = 12.12 format
	 */

    uint32_t timeout_mclks   = 0;
    uint16_t timeout_encoded = 0;

    LOG_FUNCTION_START("");

    timeout_mclks =
        VL53L1_calc_timeout_mclks(timeout_us, macro_period_us);

    timeout_encoded =
        VL53L1_encode_timeout(timeout_mclks);

#ifdef VL53L1_LOGGING
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u  (0x%04X)\n", "timeout_mclks",
                timeout_mclks, timeout_mclks);
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u  (0x%04X)\n", "timeout_encoded",
                timeout_encoded, timeout_encoded);
#endif

    LOG_FUNCTION_END(0);

    return timeout_encoded;
}


uint32_t VL53L1_calc_timeout_us(
    uint32_t timeout_mclks,
    uint32_t macro_period_us)
{
    /*  Calculates the  timeout in [us] based on the input
	 *  encoded timeout and the macro period in [us]
	 *
	 *  Max timeout supported is 1000000 us (1 sec) -> 20-bits
	 *  Max timeout in 20.12 format = 32-bits
	 *
	 *  Macro period [us] = 12.12 format
	 */

    uint32_t timeout_us = 0;
    uint64_t tmp        = 0;

    LOG_FUNCTION_START("");

    tmp  = (uint64_t)timeout_mclks * (uint64_t)macro_period_us;
    tmp += 0x00800;
    tmp  = tmp >> 12;

    timeout_us = (uint32_t)tmp;

#ifdef VL53L1_LOGGING
    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u  (0x%04X)\n", "timeout_mclks",
                timeout_mclks, timeout_mclks);

    trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                "    %-48s : %10u us\n", "timeout_us",
                timeout_us, timeout_us);
#endif

    LOG_FUNCTION_END(0);

    return timeout_us;
}

uint32_t VL53L1_calc_crosstalk_plane_offset_with_margin(
    uint32_t plane_offset_kcps,
    int16_t  margin_offset_kcps)
{
    uint32_t plane_offset_with_margin = 0;
    int32_t  plane_offset_kcps_temp   = 0;

    LOG_FUNCTION_START("");

    plane_offset_kcps_temp =
        (int32_t)plane_offset_kcps + (int32_t)margin_offset_kcps;

    if (plane_offset_kcps_temp < 0)
    {
        plane_offset_kcps_temp = 0;
    }
    else
    {
        if (plane_offset_kcps_temp > 0x3FFFF)
        {
            plane_offset_kcps_temp = 0x3FFFF;
        }
    }

    plane_offset_with_margin = (uint32_t)plane_offset_kcps_temp;

    LOG_FUNCTION_END(0);

    return plane_offset_with_margin;
}

uint32_t VL53L1_calc_decoded_timeout_us(
    uint16_t timeout_encoded,
    uint32_t macro_period_us)
{
    /*  Calculates the  timeout in [us] based on the input
	 *  encoded timeout and the macro period in [us]
	 *
	 *  Max timeout supported is 1000000 us (1 sec) -> 20-bits
	 *  Max timeout in 20.12 format = 32-bits
	 *
	 *  Macro period [us] = 12.12 format
	 */

    uint32_t timeout_mclks = 0;
    uint32_t timeout_us    = 0;

    LOG_FUNCTION_START("");

    timeout_mclks =
        VL53L1_decode_timeout(timeout_encoded);

    timeout_us =
        VL53L1_calc_timeout_us(timeout_mclks, macro_period_us);

    LOG_FUNCTION_END(0);

    return timeout_us;
}


uint16_t VL53L1_encode_timeout(uint32_t timeout_mclks)
{
    /*
	 * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
	 */

    uint16_t encoded_timeout = 0;
    uint32_t ls_byte         = 0;
    uint16_t ms_byte         = 0;

    if (timeout_mclks > 0)
    {
        ls_byte = timeout_mclks - 1;

        while ((ls_byte & 0xFFFFFF00) > 0)
        {
            ls_byte = ls_byte >> 1;
            ms_byte++;
        }

        encoded_timeout = (ms_byte << 8)
                          + (uint16_t)(ls_byte & 0x000000FF);
    }

    return encoded_timeout;
}


uint32_t VL53L1_decode_timeout(uint16_t encoded_timeout)
{
    /*
	 * Decode 16-bit timeout register value
	 * format (LSByte * 2^MSByte) + 1
	 */

    uint32_t timeout_macro_clks = 0;

    timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF)
                          << (uint32_t)((encoded_timeout & 0xFF00) >> 8))
                         + 1;

    return timeout_macro_clks;
}


VL53L1_Error VL53L1_calc_timeout_register_values(
    uint32_t                 phasecal_config_timeout_us,
    uint32_t                 mm_config_timeout_us,
    uint32_t                 range_config_timeout_us,
    uint16_t                 fast_osc_frequency,
    VL53L1_general_config_t *pgeneral,
    VL53L1_timing_config_t  *ptiming)
{
    /*
	 * Converts the input MM and range timeouts in [us]
	 * into the appropriate register values
	 *
	 * Must also be run after the VCSEL period settings are changed
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    uint32_t macro_period_us = 0;
    uint32_t timeout_mclks   = 0;
    uint16_t timeout_encoded = 0;

    LOG_FUNCTION_START("");

    if (fast_osc_frequency == 0)
    {
        status = VL53L1_ERROR_DIVISION_BY_ZERO;
    }
    else
    {
        /* Update Macro Period for Range A VCSEL Period */
        macro_period_us =
            VL53L1_calc_macro_period_us(
                fast_osc_frequency,
                ptiming->range_config__vcsel_period_a);

        /*  Update Phase timeout - uses Timing A */
        timeout_mclks =
            VL53L1_calc_timeout_mclks(
                phasecal_config_timeout_us,
                macro_period_us);

        /* clip as the phase cal timeout register is only 8-bits */
        if (timeout_mclks > 0xFF)
            timeout_mclks = 0xFF;

        pgeneral->phasecal_config__timeout_macrop =
            (uint8_t)timeout_mclks;

        /*  Update MM Timing A timeout */
        timeout_encoded =
            VL53L1_calc_encoded_timeout(
                mm_config_timeout_us,
                macro_period_us);

        ptiming->mm_config__timeout_macrop_a_hi =
            (uint8_t)((timeout_encoded & 0xFF00) >> 8);
        ptiming->mm_config__timeout_macrop_a_lo =
            (uint8_t)(timeout_encoded & 0x00FF);

        /* Update Range Timing A timeout */
        timeout_encoded =
            VL53L1_calc_encoded_timeout(
                range_config_timeout_us,
                macro_period_us);

        ptiming->range_config__timeout_macrop_a_hi =
            (uint8_t)((timeout_encoded & 0xFF00) >> 8);
        ptiming->range_config__timeout_macrop_a_lo =
            (uint8_t)(timeout_encoded & 0x00FF);

        /* Update Macro Period for Range B VCSEL Period */
        macro_period_us =
            VL53L1_calc_macro_period_us(
                fast_osc_frequency,
                ptiming->range_config__vcsel_period_b);

        /* Update MM Timing B timeout */
        timeout_encoded =
            VL53L1_calc_encoded_timeout(
                mm_config_timeout_us,
                macro_period_us);

        ptiming->mm_config__timeout_macrop_b_hi =
            (uint8_t)((timeout_encoded & 0xFF00) >> 8);
        ptiming->mm_config__timeout_macrop_b_lo =
            (uint8_t)(timeout_encoded & 0x00FF);

        /* Update Range Timing B timeout */
        timeout_encoded = VL53L1_calc_encoded_timeout(
            range_config_timeout_us,
            macro_period_us);

        ptiming->range_config__timeout_macrop_b_hi =
            (uint8_t)((timeout_encoded & 0xFF00) >> 8);
        ptiming->range_config__timeout_macrop_b_lo =
            (uint8_t)(timeout_encoded & 0x00FF);
    }

    LOG_FUNCTION_END(0);

    return status;
}


uint8_t VL53L1_encode_vcsel_period(uint8_t vcsel_period_pclks)
{
    /*
	 * Converts the encoded VCSEL period register value into
	 * the real period in PLL clocks
	 */

    uint8_t vcsel_period_reg = 0;

    vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;

    return vcsel_period_reg;
}


uint32_t VL53L1_decode_unsigned_integer(
    uint8_t *pbuffer,
    uint8_t  no_of_bytes)
{
    /*
	 * Decodes a integer number from the buffer
	 */

    uint8_t  i             = 0;
    uint32_t decoded_value = 0;

    for (i = 0; i < no_of_bytes; i++)
    {
        decoded_value = (decoded_value << 8) + (uint32_t)pbuffer[i];
    }

    return decoded_value;
}


void VL53L1_encode_unsigned_integer(
    uint32_t ip_value,
    uint8_t  no_of_bytes,
    uint8_t *pbuffer)
{
    /*
	 * Encodes an integer number into the buffer
	 */

    uint8_t  i    = 0;
    uint32_t data = 0;

    data = ip_value;
    for (i = 0; i < no_of_bytes; i++)
    {
        pbuffer[no_of_bytes - i - 1] = data & 0x00FF;
        data                         = data >> 8;
    }
}


void VL53L1_spad_number_to_byte_bit_index(
    uint8_t  spad_number,
    uint8_t *pbyte_index,
    uint8_t *pbit_index,
    uint8_t *pbit_mask)
{
    /**
     *  Converts the input SPAD number into the SPAD Enable byte index, bit index and bit mask
     *
     *  byte_index = (spad_no >> 3)
     *  bit_index  =  spad_no & 0x07
     *  bit_mask   =  0x01 << bit_index
     */

    *pbyte_index = spad_number >> 3;
    *pbit_index  = spad_number & 0x07;
    *pbit_mask   = 0x01 << *pbit_index;
}


void VL53L1_encode_row_col(
    uint8_t  row,
    uint8_t  col,
    uint8_t *pspad_number)
{
    /**
	 *  Encodes the input array(row,col) location as SPAD number.
	 */

    if (row > 7)
    {
        *pspad_number = 128 + (col << 3) + (15 - row);
    }
    else
    {
        *pspad_number = ((15 - col) << 3) + row;
    }
}


void VL53L1_decode_zone_size(
    uint8_t  encoded_xy_size,
    uint8_t *pwidth,
    uint8_t *pheight)
{
    /* extract x and y sizes
	 *
	 * Important: the sense of the device width and height is swapped
	 * versus the API sense
	 *
	 * MS Nibble = height
	 * LS Nibble = width
	 */

    *pheight = encoded_xy_size >> 4;
    *pwidth  = encoded_xy_size & 0x0F;
}


void VL53L1_encode_zone_size(
    uint8_t  width,
    uint8_t  height,
    uint8_t *pencoded_xy_size)
{
    /* merge x and y sizes
	 *
	 * Important: the sense of the device width and height is swapped
	 * versus the API sense
	 *
	 * MS Nibble = height
	 * LS Nibble = width
	 */

    *pencoded_xy_size = (height << 4) + width;
}


void VL53L1_decode_zone_limits(
    uint8_t  encoded_xy_centre,
    uint8_t  encoded_xy_size,
    int16_t *px_ll,
    int16_t *py_ll,
    int16_t *px_ur,
    int16_t *py_ur)
{
    /*
	 * compute zone lower left and upper right limits
	 *
	 * centre (8,8) width = 16, height = 16  -> (0,0) -> (15,15)
     * centre (8,8) width = 14, height = 16  -> (1,0) -> (14,15)
	 */

    uint8_t x_centre = 0;
    uint8_t y_centre = 0;
    uint8_t width    = 0;
    uint8_t height   = 0;

    /* decode zone centre and size information */

    VL53L1_decode_row_col(
        encoded_xy_centre,
        &y_centre,
        &x_centre);

    VL53L1_decode_zone_size(
        encoded_xy_size,
        &width,
        &height);

    /* compute bounds and clip */

    *px_ll = (int16_t)x_centre - ((int16_t)width + 1) / 2;
    if (*px_ll < 0)
        *px_ll = 0;

    *px_ur = *px_ll + (int16_t)width;
    if (*px_ur > (VL53L1_SPAD_ARRAY_WIDTH - 1))
        *px_ur = VL53L1_SPAD_ARRAY_WIDTH - 1;

    *py_ll = (int16_t)y_centre - ((int16_t)height + 1) / 2;
    if (*py_ll < 0)
        *py_ll = 0;

    *py_ur = *py_ll + (int16_t)height;
    if (*py_ur > (VL53L1_SPAD_ARRAY_HEIGHT - 1))
        *py_ur = VL53L1_SPAD_ARRAY_HEIGHT - 1;
}


uint8_t VL53L1_is_aperture_location(
    uint8_t row,
    uint8_t col)
{
    /*
	 * Returns > 0 if input (row,col) location  is an aperture
	 */

    uint8_t is_aperture = 0;
    uint8_t mod_row     = row % 4;
    uint8_t mod_col     = col % 4;

    if (mod_row == 0 && mod_col == 2)
        is_aperture = 1;

    if (mod_row == 2 && mod_col == 0)
        is_aperture = 1;

    return is_aperture;
}


void VL53L1_calc_mm_effective_spads(
    uint8_t   encoded_mm_roi_centre,
    uint8_t   encoded_mm_roi_size,
    uint8_t   encoded_zone_centre,
    uint8_t   encoded_zone_size,
    uint8_t  *pgood_spads,
    uint16_t  aperture_attenuation,
    uint16_t *pmm_inner_effective_spads,
    uint16_t *pmm_outer_effective_spads)
{
    /* Calculates the effective SPAD counts for the MM inner and outer
	 * regions based on the input MM ROI, Zone info and return good
	 * SPAD map
	 */

    int16_t x = 0;
    int16_t y = 0;

    int16_t mm_x_ll = 0;
    int16_t mm_y_ll = 0;
    int16_t mm_x_ur = 0;
    int16_t mm_y_ur = 0;

    int16_t zone_x_ll = 0;
    int16_t zone_y_ll = 0;
    int16_t zone_x_ur = 0;
    int16_t zone_y_ur = 0;

    uint8_t spad_number = 0;
    uint8_t byte_index  = 0;
    uint8_t bit_index   = 0;
    uint8_t bit_mask    = 0;

    uint8_t  is_aperture      = 0;
    uint16_t spad_attenuation = 0;

    /* decode the MM ROI and Zone limits */

    VL53L1_decode_zone_limits(
        encoded_mm_roi_centre,
        encoded_mm_roi_size,
        &mm_x_ll,
        &mm_y_ll,
        &mm_x_ur,
        &mm_y_ur);

    VL53L1_decode_zone_limits(
        encoded_zone_centre,
        encoded_zone_size,
        &zone_x_ll,
        &zone_y_ll,
        &zone_x_ur,
        &zone_y_ur);

    /*
	 * Loop though all SPAD within the zone. Check if it is
	 * a good SPAD then add the  transmission value to either
	 * the inner or outer effective SPAD count dependent if
	 * the SPAD lies within the MM ROI.
	 */

    *pmm_inner_effective_spads = 0;
    *pmm_outer_effective_spads = 0;

    for (y = zone_y_ll; y <= zone_y_ur; y++)
    {
        for (x = zone_x_ll; x <= zone_x_ur; x++)
        {
            /* Convert location into SPAD number */

            VL53L1_encode_row_col(
                (uint8_t)y,
                (uint8_t)x,
                &spad_number);

            /* Convert spad number into byte and bit index
			 * this is required to look up the appropriate
			 * SPAD enable bit with the 32-byte good SPAD
			 * enable buffer
			 */

            VL53L1_spad_number_to_byte_bit_index(
                spad_number,
                &byte_index,
                &bit_index,
                &bit_mask);

            /* If spad is good then add it */

            if ((pgood_spads[byte_index] & bit_mask) > 0)
            {
                /* determine if apertured SPAD or not */

                is_aperture = VL53L1_is_aperture_location(
                    (uint8_t)y,
                    (uint8_t)x);

                if (is_aperture > 0)
                    spad_attenuation = aperture_attenuation;
                else
                    spad_attenuation = 0x0100;

                /*
				 * if inside MM roi add to inner effective SPAD count
				 * otherwise add to outer effective SPAD Count
				 */

                if (x >= mm_x_ll && x <= mm_x_ur && y >= mm_y_ll && y <= mm_y_ur)
                    *pmm_inner_effective_spads +=
                        spad_attenuation;
                else
                    *pmm_outer_effective_spads +=
                        spad_attenuation;
            }
        }
    }
}


/*
 * Encodes VL53L1_GPIO_interrupt_config_t structure to FW register format
 */

uint8_t VL53L1_encode_GPIO_interrupt_config(
    VL53L1_GPIO_interrupt_config_t *pintconf)
{
    uint8_t system__interrupt_config;

    system__interrupt_config  = pintconf->intr_mode_distance;
    system__interrupt_config |= ((pintconf->intr_mode_rate) << 2);
    system__interrupt_config |= ((pintconf->intr_new_measure_ready) << 5);
    system__interrupt_config |= ((pintconf->intr_no_target) << 6);
    system__interrupt_config |= ((pintconf->intr_combined_mode) << 7);

    return system__interrupt_config;
}

/*
 * Decodes FW register to VL53L1_GPIO_interrupt_config_t structure
 */

VL53L1_GPIO_interrupt_config_t VL53L1_decode_GPIO_interrupt_config(
    uint8_t system__interrupt_config)
{
    VL53L1_GPIO_interrupt_config_t intconf;

    intconf.intr_mode_distance     = system__interrupt_config & 0x03;
    intconf.intr_mode_rate         = (system__interrupt_config >> 2) & 0x03;
    intconf.intr_new_measure_ready = (system__interrupt_config >> 5) & 0x01;
    intconf.intr_no_target         = (system__interrupt_config >> 6) & 0x01;
    intconf.intr_combined_mode     = (system__interrupt_config >> 7) & 0x01;

    /* set some default values */
    intconf.threshold_rate_low      = 0;
    intconf.threshold_rate_high     = 0;
    intconf.threshold_distance_low  = 0;
    intconf.threshold_distance_high = 0;

    return intconf;
}

/*
 * Set GPIO distance threshold
 */

VL53L1_Error VL53L1_set_GPIO_distance_threshold(
    VL53L1_DEV Dev,
    uint16_t   threshold_high,
    uint16_t   threshold_low)
{
    VL53L1_Error status = VL53L1_ERROR_NONE;

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    pdev->dyn_cfg.system__thresh_high = threshold_high;
    pdev->dyn_cfg.system__thresh_low  = threshold_low;

    LOG_FUNCTION_END(status);
    return status;
}

/*
 * Set GPIO rate threshold
 */

VL53L1_Error VL53L1_set_GPIO_rate_threshold(
    VL53L1_DEV Dev,
    uint16_t   threshold_high,
    uint16_t   threshold_low)
{
    VL53L1_Error status = VL53L1_ERROR_NONE;

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    pdev->gen_cfg.system__thresh_rate_high = threshold_high;
    pdev->gen_cfg.system__thresh_rate_low  = threshold_low;

    LOG_FUNCTION_END(status);
    return status;
}

/*
 * Set GPIO thresholds from structure
 */

VL53L1_Error VL53L1_set_GPIO_thresholds_from_struct(
    VL53L1_DEV                      Dev,
    VL53L1_GPIO_interrupt_config_t *pintconf)
{
    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    status = VL53L1_set_GPIO_distance_threshold(
        Dev,
        pintconf->threshold_distance_high,
        pintconf->threshold_distance_low);

    if (status == VL53L1_ERROR_NONE)
    {
        status =
            VL53L1_set_GPIO_rate_threshold(
                Dev,
                pintconf->threshold_rate_high,
                pintconf->threshold_rate_low);
    }

    LOG_FUNCTION_END(status);
    return status;
}


#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_set_ref_spad_char_config(
    VL53L1_DEV Dev,
    uint8_t    vcsel_period_a,
    uint32_t   phasecal_timeout_us,
    uint16_t   total_rate_target_mcps,
    uint16_t   max_count_rate_rtn_limit_mcps,
    uint16_t   min_count_rate_rtn_limit_mcps,
    uint16_t   fast_osc_frequency)
{
    /*
	 * Initialises the VCSEL period A and phasecal timeout registers
	 * for the Reference SPAD Characterisation test
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    uint8_t buffer[2];

    uint32_t macro_period_us = 0;
    uint32_t timeout_mclks   = 0;

    LOG_FUNCTION_START("");

    /*
	 * Update Macro Period for Range A VCSEL Period
	 */
    macro_period_us =
        VL53L1_calc_macro_period_us(
            fast_osc_frequency,
            vcsel_period_a);

    /*
	 *  Calculate PhaseCal timeout and clip to max of 255 macro periods
	 */

    timeout_mclks = phasecal_timeout_us << 12;
    timeout_mclks = timeout_mclks + (macro_period_us >> 1);
    timeout_mclks = timeout_mclks / macro_period_us;

    if (timeout_mclks > 0xFF)
        pdev->gen_cfg.phasecal_config__timeout_macrop = 0xFF;
    else
        pdev->gen_cfg.phasecal_config__timeout_macrop =
            (uint8_t)timeout_mclks;

    pdev->tim_cfg.range_config__vcsel_period_a = vcsel_period_a;

    /*
	 * Update device settings
	 */

    if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
        status =
            VL53L1_WrByte(
                Dev,
                VL53L1_PHASECAL_CONFIG__TIMEOUT_MACROP,
                pdev->gen_cfg.phasecal_config__timeout_macrop);

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrByte(
                Dev,
                VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A,
                pdev->tim_cfg.range_config__vcsel_period_a);

    /*
	 * Copy vcsel register value to the WOI registers to ensure that
	 * it is correctly set for the specified VCSEL period
	 */

    buffer[0] = pdev->tim_cfg.range_config__vcsel_period_a;
    buffer[1] = pdev->tim_cfg.range_config__vcsel_period_a;

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WriteMulti(
                Dev,
                VL53L1_SD_CONFIG__WOI_SD0,
                buffer,
                2); /* It should be be replaced with a define */

    /*
	 * Set min, target and max rate limits
	 */

    pdev->customer.ref_spad_char__total_rate_target_mcps =
        total_rate_target_mcps;

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrWord(
                Dev,
                VL53L1_REF_SPAD_CHAR__TOTAL_RATE_TARGET_MCPS,
                total_rate_target_mcps); /* 9.7 format */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrWord(
                Dev,
                VL53L1_RANGE_CONFIG__SIGMA_THRESH,
                max_count_rate_rtn_limit_mcps);

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrWord(
                Dev,
                VL53L1_RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,
                min_count_rate_rtn_limit_mcps);

    LOG_FUNCTION_END(status);

    return status;
}


VL53L1_Error VL53L1_set_ssc_config(
    VL53L1_DEV           Dev,
    VL53L1_ssc_config_t *pssc_cfg,
    uint16_t             fast_osc_frequency)
{
    /**
	 * Builds and sends a single I2C multiple byte transaction to
	 * initialize the device for SSC.
	 *
	 * The function also sets the WOI registers based on the input
	 * vcsel period register value.
	 */

    VL53L1_Error status = VL53L1_ERROR_NONE;
    uint8_t      buffer[5];

    uint32_t macro_period_us = 0;
    uint16_t timeout_encoded = 0;

    LOG_FUNCTION_START("");

    /*
	 * Update Macro Period for Range A VCSEL Period
	 */
    macro_period_us =
        VL53L1_calc_macro_period_us(
            fast_osc_frequency,
            pssc_cfg->vcsel_period);

    /*
	 *  Update MM Timing A timeout
	 */
    timeout_encoded =
        VL53L1_calc_encoded_timeout(
            pssc_cfg->timeout_us,
            macro_period_us);

    /* update VCSEL timings */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrByte(
                Dev,
                VL53L1_CAL_CONFIG__VCSEL_START,
                pssc_cfg->vcsel_start);

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrByte(
                Dev,
                VL53L1_GLOBAL_CONFIG__VCSEL_WIDTH,
                pssc_cfg->vcsel_width);

    /* build buffer for timeouts, period and rate limit */

    buffer[0] = (uint8_t)((timeout_encoded & 0x0000FF00) >> 8);
    buffer[1] = (uint8_t)(timeout_encoded & 0x000000FF);
    buffer[2] = pssc_cfg->vcsel_period;
    buffer[3] = (uint8_t)((pssc_cfg->rate_limit_mcps & 0x0000FF00) >> 8);
    buffer[4] = (uint8_t)(pssc_cfg->rate_limit_mcps & 0x000000FF);

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WriteMulti(
                Dev,
                VL53L1_RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                buffer,
                5);

    /*
	 * Copy vcsel register value to the WOI registers to ensure that
	 * it is correctly set for the specified VCSEL period
	 */

    buffer[0] = pssc_cfg->vcsel_period;
    buffer[1] = pssc_cfg->vcsel_period;

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WriteMulti(
                Dev,
                VL53L1_SD_CONFIG__WOI_SD0,
                buffer,
                2);

    /*
	 * Write zero to NVM_BIST_CTRL to send RTN CountRate to Patch RAM
	 * or 1 to write REF CountRate to Patch RAM
	 */
    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WrByte(
                Dev,
                VL53L1_NVM_BIST__CTRL,
                pssc_cfg->array_select);

    LOG_FUNCTION_END(status);

    return status;
}
#endif


#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_get_spad_rate_data(
    VL53L1_DEV               Dev,
    VL53L1_spad_rate_data_t *pspad_rates)
{
    /**
     *  Gets the SSC rate map output
     */

    VL53L1_Error status = VL53L1_ERROR_NONE;
    int          i      = 0;

    uint8_t  data[512];
    uint8_t *pdata = &data[0];

    LOG_FUNCTION_START("");

    /* Disable Firmware to Read Patch Ram */

    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_disable_firmware(Dev);

    /*
     * Read Return SPADs Rates from patch RAM.
     * Note : platform layer  splits the I2C comms into smaller chunks
     */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_ReadMulti(
                Dev,
                VL53L1_PRIVATE__PATCH_BASE_ADDR_RSLV,
                pdata,
                512);

    /* now convert into 16-bit number */
    pdata = &data[0];
    for (i = 0; i < VL53L1_NO_OF_SPAD_ENABLES; i++)
    {
        pspad_rates->rate_data[i] =
            (uint16_t)VL53L1_decode_unsigned_integer(pdata, 2);
        pdata += 2;
    }

    /* Initialise structure info */

    pspad_rates->buffer_size     = VL53L1_NO_OF_SPAD_ENABLES;
    pspad_rates->no_of_values    = VL53L1_NO_OF_SPAD_ENABLES;
    pspad_rates->fractional_bits = 15;

    /* Re-enable Firmware */

    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_enable_firmware(Dev);

    LOG_FUNCTION_END(status);

    return status;
}
#endif

/* Start Patch_LowPowerAutoMode */

VL53L1_Error VL53L1_low_power_auto_data_init(
    VL53L1_DEV Dev)
{
    /*
	 * Initializes internal data structures for low power auto mode
	 */

    /* don't really use this here */
    VL53L1_Error status = VL53L1_ERROR_NONE;

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    pdev->low_power_auto_data.vhv_loop_bound =
        VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT;
    pdev->low_power_auto_data.is_low_power_auto_mode        = 0;
    pdev->low_power_auto_data.low_power_auto_range_count    = 0;
    pdev->low_power_auto_data.saved_interrupt_config        = 0;
    pdev->low_power_auto_data.saved_vhv_init                = 0;
    pdev->low_power_auto_data.saved_vhv_timeout             = 0;
    pdev->low_power_auto_data.first_run_phasecal_result     = 0;
    pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
    pdev->low_power_auto_data.dss__required_spads           = 0;

    LOG_FUNCTION_END(status);

    return status;
}

VL53L1_Error VL53L1_low_power_auto_data_stop_range(
    VL53L1_DEV Dev)
{
    /*
	 * Range has been paused but may continue later
	 */

    /* don't really use this here */
    VL53L1_Error status = VL53L1_ERROR_NONE;

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    /* doing this ensures stop_range followed by a get_device_results does
	 * not mess up the counters */

    pdev->low_power_auto_data.low_power_auto_range_count = 0xFF;

    pdev->low_power_auto_data.first_run_phasecal_result     = 0;
    pdev->low_power_auto_data.dss__total_rate_per_spad_mcps = 0;
    pdev->low_power_auto_data.dss__required_spads           = 0;

    /* restore vhv configs */
    if (pdev->low_power_auto_data.saved_vhv_init != 0)
        pdev->stat_nvm.vhv_config__init =
            pdev->low_power_auto_data.saved_vhv_init;
    if (pdev->low_power_auto_data.saved_vhv_timeout != 0)
        pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
            pdev->low_power_auto_data.saved_vhv_timeout;

    /* remove phasecal override */
    pdev->gen_cfg.phasecal_config__override = 0x00;

    LOG_FUNCTION_END(status);

    return status;
}

VL53L1_Error VL53L1_config_low_power_auto_mode(
    VL53L1_general_config_t      *pgeneral,
    VL53L1_dynamic_config_t      *pdynamic,
    VL53L1_low_power_auto_data_t *plpadata)
{
    /*
	 * Initializes configs for when low power auto presets are selected
	 */

    /* don't really use this here */
    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    /* set low power auto mode */
    plpadata->is_low_power_auto_mode = 1;

    /* set low power range count to 0 */
    plpadata->low_power_auto_range_count = 0;

    /* Turn off MM1/MM2 and DSS2 */
    pdynamic->system__sequence_config =
        VL53L1_SEQUENCE_VHV_EN | VL53L1_SEQUENCE_PHASECAL_EN | VL53L1_SEQUENCE_DSS1_EN | /* VL53L1_SEQUENCE_DSS2_EN | \*/
        /* VL53L1_SEQUENCE_MM1_EN | \*/
        /* VL53L1_SEQUENCE_MM2_EN | \*/
        VL53L1_SEQUENCE_RANGE_EN;

    /* Set DSS to manual/expected SPADs */
    pgeneral->dss_config__manual_effective_spads_select = 200 << 8;
    pgeneral->dss_config__roi_mode_control =
        VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;

    LOG_FUNCTION_END(status);

    return status;
}

VL53L1_Error VL53L1_low_power_auto_setup_manual_calibration(
    VL53L1_DEV Dev)
{
    /*
	 * Setup ranges after the first one in low power auto mode by turning
	 * off FW calibration steps and programming static values
	 */

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    /* don't really use this here */
    VL53L1_Error status = VL53L1_ERROR_NONE;

    LOG_FUNCTION_START("");

    /* save original vhv configs */
    pdev->low_power_auto_data.saved_vhv_init =
        pdev->stat_nvm.vhv_config__init;
    pdev->low_power_auto_data.saved_vhv_timeout =
        pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound;

    /* disable VHV init */
    pdev->stat_nvm.vhv_config__init &= 0x7F;
    /* set loop bound to tuning param */
    pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound =
        (pdev->stat_nvm.vhv_config__timeout_macrop_loop_bound & 0x03) + (pdev->low_power_auto_data.vhv_loop_bound << 2);
    /* override phasecal */
    pdev->gen_cfg.phasecal_config__override = 0x01;
    pdev->low_power_auto_data.first_run_phasecal_result =
        pdev->dbg_results.phasecal_result__vcsel_start;
    pdev->gen_cfg.cal_config__vcsel_start =
        pdev->low_power_auto_data.first_run_phasecal_result;

    LOG_FUNCTION_END(status);

    return status;
}

VL53L1_Error VL53L1_low_power_auto_update_DSS(
    VL53L1_DEV Dev)
{
    /*
	 * Do a DSS calculation and update manual config
	 */

    VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);

    /* don't really use this here */
    VL53L1_Error status = VL53L1_ERROR_NONE;

    uint32_t utemp32a;

    LOG_FUNCTION_START("");

    /* Calc total rate per spad */

    /* 9.7 format */
    utemp32a = pdev->sys_results.result__peak_signal_count_rate_crosstalk_corrected_mcps_sd0 + pdev->sys_results.result__ambient_count_rate_mcps_sd0;

    /* clip to 16 bits */
    if (utemp32a > 0xFFFF)
        utemp32a = 0xFFFF;

    /* shift up to take advantage of 32 bits */
    /* 9.23 format */
    utemp32a = utemp32a << 16;

    /* check SPAD count */
    if (pdev->sys_results.result__dss_actual_effective_spads_sd0 == 0)
        status = VL53L1_ERROR_DIVISION_BY_ZERO;
    else
    {
        /* format 17.15 */
        utemp32a = utemp32a / pdev->sys_results.result__dss_actual_effective_spads_sd0;
        /* save intermediate result */
        pdev->low_power_auto_data.dss__total_rate_per_spad_mcps =
            utemp32a;

        /* get the target rate and shift up by 16
		 * format 9.23 */
        utemp32a = pdev->stat_cfg.dss_config__target_total_rate_mcps << 16;

        /* check for divide by zero */
        if (pdev->low_power_auto_data.dss__total_rate_per_spad_mcps == 0)
            status = VL53L1_ERROR_DIVISION_BY_ZERO;
        else
        {
            /* divide by rate per spad
			 * format 24.8 */
            utemp32a = utemp32a / pdev->low_power_auto_data.dss__total_rate_per_spad_mcps;

            /* clip to 16 bit */
            if (utemp32a > 0xFFFF)
                utemp32a = 0xFFFF;

            /* save result in low power auto data */
            pdev->low_power_auto_data.dss__required_spads =
                (uint16_t)utemp32a;

            /* override DSS config */
            pdev->gen_cfg.dss_config__manual_effective_spads_select =
                pdev->low_power_auto_data.dss__required_spads;
            pdev->gen_cfg.dss_config__roi_mode_control =
                VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;
        }
    }

    if (status == VL53L1_ERROR_DIVISION_BY_ZERO)
    {
        /* We want to gracefully set a spad target, not just exit with
		* an error */

        /* set target to mid point */
        pdev->low_power_auto_data.dss__required_spads = 0x8000;

        /* override DSS config */
        pdev->gen_cfg.dss_config__manual_effective_spads_select =
            pdev->low_power_auto_data.dss__required_spads;
        pdev->gen_cfg.dss_config__roi_mode_control =
            VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS;

        /* reset error */
        status = VL53L1_ERROR_NONE;
    }

    LOG_FUNCTION_END(status);

    return status;
}


/* End Patch_LowPowerAutoMode */
